
#ifndef __GLOBAL_PATH_PLANNER_SMOOTH_PATH_H__
#define __GLOBAL_PATH_PLANNER_SMOOTH_PATH_H__

#include "discretized_points_smoother/fem_pos_deviation_smoother.h"
#include "Pose2D.h"
#include "DF.h"
#include "glog_func/log.h"

namespace global_path_planner {
namespace path_smooth {

/**
 * @brief 使用fem_pos对path(均匀采样的离散点路径)做平滑 (首尾点不动, 其余路点的x、y变动范围小于等于bound)
 */
inline bool SmoothPoseDq(PoseDq& path, const double bound) {
    if (path.size() < 2) {
        return false;
    }
    fem_smooth::FemPosDeviationSmootherConfig fem_pos_config;
    fem_smooth::FemPosDeviationSmoother fem_pos_smoother(fem_pos_config);
    std::vector<std::pair<double, double>> raw_points;
    std::vector<double> bounds(path.size(), bound);
    std::vector<double> xopts;
    std::vector<double> yopts;
    for (size_t i = 0; i < path.size(); ++i) {
        raw_points.emplace_back(path.at(i).X(), path.at(i).Y());
    }
    bounds.front() = 0.0;
    bounds.back() = 0.0;
    bool status = fem_pos_smoother.Solve(raw_points, bounds, &xopts, &yopts);
    if (status) {
        for (size_t k = 0; k < xopts.size(); ++k) {
            path.at(k).SetX(xopts.at(k));
            path.at(k).SetY(yopts.at(k));
        }
        // LOG(INFO) << "fem_pos smooth succeed !";
    } else {
        LOG(WARNING) << "fem_pos smooth fail !";
    }
    return status;
}

/**
 * @brief 微调路径上安全余量不足的路径段
 * @param path 待微调的路径
 * @param df_map 障碍物距离场地图 (最近障碍物栅格距离的平方)
 * @param expected_dis 期望的障碍物距离, unit: m
 * @return true path需要微调且已微调
 * @return false path不需要微调
 */
inline bool ProcessRiskRanges(PoseDq& path,
                              const ::common::planning::DistanceMap& df_map,
                              const double expected_dis) {
    if (path.size() <= 2) { // 非法输入
        return false;
    }

    // LOG(INFO) << "expected_dis: " << expected_dis;
    double expected_grid_dis = expected_dis / df_map.Resolution();
    double thresh_df_dis = expected_grid_dis * expected_grid_dis; // 栅格距离的平方

    // TODO: rj.wang GetGrid没有做越界检查
    std::vector<std::pair<size_t, size_t>> risk_ranges{}; // 用以标记调整区间

    // 路点微调
    auto tmp_path = path;
    for (size_t i = 1; i+1 < path.size(); ++i) { // 首尾不调整
        auto& p_i = path.at(i);
        auto obs_dist = df_map.GetGrid(df_map.X2IX(p_i.X()), df_map.Y2IY(p_i.Y()));
        if (obs_dist > thresh_df_dis) { // 满足要求, 不需要调整
            continue;
        }

        // 检查微调是否可以获得更安全的路点
        auto phi = atan2(tmp_path.at(i+1).Y() - tmp_path.at(i-1).Y(), tmp_path.at(i+1).X() - tmp_path.at(i-1).X()); // 计算朝向
        bool better_pose_exist = false;
        using namespace ::common::base;
        double left_x = p_i.X() + df_map.Resolution() * cos(Angle16::from_rad(phi + M_PI_2));
        double left_y = p_i.Y() + df_map.Resolution() * sin(Angle16::from_rad(phi + M_PI_2));
        auto left_obs_dist = df_map.GetGrid(df_map.X2IX(left_x), df_map.Y2IY(left_y));  // 往左微调一个栅格分辨率
        double right_x = p_i.X() + df_map.Resolution() * cos(Angle16::from_rad(phi - M_PI_2));
        double right_y = p_i.Y() + df_map.Resolution() * sin(Angle16::from_rad(phi - M_PI_2));
        auto right_obs_dist = df_map.GetGrid(df_map.X2IX(right_x), df_map.Y2IY(right_y));  // 往右微调一个栅格分辨率
        if (left_obs_dist > right_obs_dist && left_obs_dist > obs_dist + 1.0) {
            better_pose_exist = true;
            p_i.SetX(left_x);
            p_i.SetY(left_y); // 局部微调
        } else if (right_obs_dist > left_obs_dist && right_obs_dist > obs_dist + 1.0) {
            better_pose_exist = true;
            p_i.SetX(right_x);
            p_i.SetY(right_y); // 局部微调
        }
        if (better_pose_exist) {  // 微调可以获取更安全的路点
            if (risk_ranges.empty() || i > risk_ranges.back().second + 1) {
                risk_ranges.emplace_back(i, i);
            } else {
                risk_ranges.back().second = i;
            }
        }
    }

    // 微调后做路径平滑
    if (!risk_ranges.empty() && path.size() > 2) {
        fem_smooth::FemPosDeviationSmootherConfig fem_pos_config;
        fem_smooth::FemPosDeviationSmoother fem_pos_smoother(fem_pos_config);
        std::vector<std::pair<double, double>> raw_points;
        for (size_t i = 0; i < path.size(); ++i) {
            raw_points.emplace_back(path.at(i).X(), path.at(i).Y());
        }
        std::vector<double> bounds(path.size(), 0.0);
        const size_t kExtendRange = 5; // 平滑扩张范围
        for (const auto& range : risk_ranges) {
            size_t start_range = range.first > kExtendRange ? range.first - kExtendRange : 0;
            size_t end_range = range.second + kExtendRange < bounds.size() ? range.second + kExtendRange : bounds.size()-1;
            for (size_t i = start_range; i <= end_range; ++ i) {
                bounds.at(i) = 0.5 * df_map.Resolution(); // 路点微调距离的一半
            }
        }
        bounds.front() = 0.0;
        bounds.back() = 0.0; // 首尾不调整
        std::vector<double> xopts;
        std::vector<double> yopts;
        bool status = fem_pos_smoother.Solve(raw_points, bounds, &xopts, &yopts); // 平滑
        if (status) {
            for (size_t k = 0; k < xopts.size(); ++k) {
                path.at(k).SetX(xopts.at(k));
                path.at(k).SetY(yopts.at(k));
            }
            // LOG(INFO) << "fem_pos smooth succeed !";
        } else {
            LOG(WARNING) << "fem_pos smooth fail !";
        }
    }    

    return !risk_ranges.empty();
}

}  // namespace path_smooth
}  // namespace global_path_planner

#endif // __GLOBAL_PATH_PLANNER_SMOOTH_PATH_H__